import pinocchio as pin
import meshcat
import numpy as np
from os.path import dirname, join
from pinocchio.visualize import MeshcatVisualizer

URDF_FILE_PATH = join(dirname(dirname(__file__)),'Dof6Arm.xml')

robot = pin.RobotWrapper.BuildFromURDF(URDF_FILE_PATH, [dirname(URDF_FILE_PATH)])
viz = MeshcatVisualizer(robot.model, robot.collision_model, robot.visual_model)
viz.initViewer(open=True, loadModel=True)